#include "heatRiseTest.h"
#include "amLog.h"
#include "globalVariable.h"
#include "board_id.h"
#include "wl_main.h"
#include "sensorProc.h"

#if 0
float tempCacluByseif(uint16_t data)
{
    //float temp=0;
    //float temperate;
    //uint16_t ts_cal1,ts_cal2;

//    ts_cal1=*(uint16_t*)(0X1FF1E820);
//    ts_cal2=*(uint16_t*)(0X1FF1E840);
//    temp=(float)((110.0f-30.0f)/(ts_cal2-ts_cal1));		   
//    temperate=(float)(temp*(data-ts_cal1)+30);
    //return temperate;
}
#endif

void temperatureAndSystemCoreClockOutput(void) 
{
    LOG_TS(AM_LOG_LEVEL_INFO, "MCU State", "temp:%d ,mcu_Hz:%d ,freeheap:%d\n",
                                40,  SystemCoreClock/1000, xPortGetFreeHeapSize());               //disable ecu ntc channel
}

void testCPURunningAlgorithm(void)
{

}


 
//求解fibonacci数列
//朴素方法
int Simple_Fibonacci(int n)
{
    if(n<=0){
        return 0;//避免非法输入
    }
    if(n==1){
        return 1;
    }
    else{
        return Simple_Fibonacci(n-1) + Simple_Fibonacci(n-2);
    }
}

/*
 * 同时 fibonacci还可以使用通项公式求取
 */
int fib(int n) 
{
    double sqrt5 = sqrt(5);
    double fibN = pow((1 + sqrt5) / 2, n) - pow((1 - sqrt5) / 2, n);
    return round(fibN / sqrt5);
}



float RandomNumGenerator(int base, int range)
{
	float k = 0.0;
	float randomNum = 0.0;
	k = 2 * range * 10;
	randomNum = rand() % (int)k;
	k = base - range + (randomNum / 10);
	return k;
}

//void BoostRandomNumGenerator() 
//{
//	srand((unsigned)time(NULL));
//}

void Kalman_Init(KalmanFilter* kalmanFilter, float FirstMeaValue, float E_mea, float FirstEstValue, float E_est) {
	kalmanFilter->x_est = FirstEstValue;
	kalmanFilter->x_mea = FirstMeaValue;
	kalmanFilter->e_est = E_est;
	kalmanFilter->e_mea = E_mea;
	kalmanFilter->Kk = Kk_calc(kalmanFilter->e_est, kalmanFilter->e_mea);
}

void Kalman_Update(KalmanFilter* kalmanFilter, float newMeaValue) {
	float temp = kalmanFilter->e_est;
	kalmanFilter->x_est = kalmanFilter->x_est + kalmanFilter->Kk * (newMeaValue - kalmanFilter->x_est);
	kalmanFilter->x_mea = newMeaValue;
	kalmanFilter->Kk = Kk_calc(kalmanFilter->e_est, kalmanFilter->e_mea);
	kalmanFilter->e_est = (1 - kalmanFilter->Kk) * temp;
}

int kalmanFilterAlgorithm(void)
{
	KalmanFilter k;
	//BoostRandomNumGenerator();
	Kalman_Init(&k, 51.0, 3.0, 40, 5);
	for (int i = 0; i < 10; i++)
	{
	    // Ten iterations
		Kalman_Update(&k, RandomNumGenerator(50, 3));
		printf("%.3f | %.3f\n",k.x_mea,k.x_est);
	}
	return 0;
}
#define R2D         (180.0/PI)          /* rad to deg */
#define PI          3.1415926535897932  /* pi */
#define RE_WGS84    6378137.0           /* earth semimajor axis (WGS84) (m) */
#define FE_WGS84    (1.0/298.257223563) /* earth flattening (WGS84) */

extern double dot1(const double *a, const double *b, int n)
{
    double c=0.0;
    
    while (--n>=0) c+=a[n]*b[n];
    return c;
}
extern void ecef2pos1(const double* r, double* pos)
{
    double e2 = FE_WGS84 * (2.0 - FE_WGS84), r2 = dot1(r, r, 2), z, zk, v = RE_WGS84, sinp;

    for (z = r[2], zk = 0.0; fabs(z - zk) >= 1E-4;) {
        zk = z;
        sinp = z / sqrt(r2 + z * z);
        v = RE_WGS84 / sqrt(1.0 - e2 * sinp * sinp);
        z = r[2] + v * e2 * sinp;
    }
    pos[0] = r2 > 1E-12 ? atan(z / sqrt(r2)) : (r[2] > 0.0 ? PI / 2.0 : -PI / 2.0);
    pos[1] = r2 > 1E-12 ? atan2(r[1], r[0]) : 0.0;
    pos[2] = sqrt(r2 + z * z) - v;
}

void test()
{
    double pos[3], ecef[3] = { -2170726.7382767387,4381707.9124369379,4081344.9866223736 };
    while (1)
    {
        for (int i = 0; i < 10000; i++)
        {
            for (int j = 0; j < 3; j++) ecef[j] += 0.01;
            ecef2pos1(ecef,pos);
            //printf("%.6f %.6f %.3f\n", pos[0]*R2D, pos[1]*R2D, pos[2]);
        }
        for (int i = 0; i < 10000; i++)
        {
            for (int j = 0; j < 3; j++) ecef[j] -= 0.01;
            ecef2pos1(ecef,pos);
            //printf("%.6f %.6f %.3f\n", pos[0]*R2D, pos[1]*R2D, pos[2]);
        }
    }
}
